#pragma once
#include <memory>
#include "nml_intf/zucpos2.h"
#include "fct_para_common.h"
namespace axisgroup {

class Robot;
namespace ForceControl {
    class ForceSensor
    {
    public:
        ForceSensor();
        int brand_ = -1;
        bool is_opened_ = false;
        enum ErrorState
        {
            ERR_NONE = 0,
            ERR_INIT_ERROR = 1,
            ERR_RECV_TIMEOUT = 2,
        } err_state_ = ERR_NONE;
        double raw_data_[6] = {};
        double actual_data_[6] = {};
        int error_data_count_ = 0;

        const static int SENSOR_SEM_KEY = 2002;
        SensorShm* sensor_shm_ = nullptr;

        void update();
    };
};  // namespace ForceControl

class ForceControlIntf
{
public:
    ForceControlIntf(Robot* grp);
    void enable_force_control(const ZucPose& cur_pose);
    void disable_force_control();
    int admittanceRevise(const ZucPose& desired_pose, AxisGroupPose& cur_robot_pose, ZucPose& revised_pose);
    int admittanceReviseCombinedDrag(const ZucPose& desired_pose, AxisGroupPose& cur_robot_pose, ZucPose& revised_pose);
    int admittanceReviseForDrag(const ZucPose& desired_pose, AxisGroupPose& cur_robot_pose, ZucPose& revised_pose, const double* End_Force_comb);
    FCAxisPara fc_config_;
    ForceControl::ForceSensor force_sensor_;

public:
    Robot* grp_ = nullptr;
    bool is_drag_ = false;
    // 力控相关
    bool last_compliant_enable_ = 0;
    bool compliant_enable_state_ = 0;
    bool commnand_to_close_ = 0;

    AxisGroupPose last_force_control_pose_;

private:
    void zucpose_to_double(const ZucPose& pose, double* data, bool to_rad);  // zucpose 中的度会被转化为弧度
    void double_to_zucpose(const double* data, ZucPose& pose, bool to_deg);  // double 中的弧度会被转化为度
};

}  // namespace axisgroup
